import networkx as nx
import matplotlib.pyplot as plt
from Split_equally import split_path_equally


class AGV:
    def __init__(self, id, start, end):
        self.id = id
        self.start = start
        self.end = end
        self.path = []

def draw_graph(G,start,end, paths=None, title="多AGV路径规划"):
    pos = {node: (node[0], node[1]) for node in G.nodes()}
    plt.figure(figsize=(3,8))
    nx.draw(G, pos,  node_size=0.2, node_color='lightblue', font_size=8, arrowstyle='-', arrowsize=2)
    step_size = 1
    pathss=[]
    # print(paths)
    for path in paths:
        segmented_path = split_path_equally(path, step_size)
        pathss.append(segmented_path)
    print(pathss)
    plt.ion()
    if pathss:
        for path in pathss:
            i=pathss.index(path)
            # print(i)
            path_edge=paths[i]
            edges = [(path_edge[i], path_edge[i + 1]) for i in range(len(path_edge) - 1)]
            # print(edges)
            nx.draw(G, pos,node_size=0.2, node_color='lightblue', font_size=8, arrowstyle='-', arrowsize=5,
                    edgelist=edges, edge_color='r', width=2)
            plt.scatter(start[i][0], start[i][1], c='r')
            plt.scatter(end[i][0], end[i][1], c='r')

        a = []
        b = []
        length=len(pathss)
        # print(length)
        # print(max(len(path) for path in pathss)-1)

        for i in range(length):
            a.append([])
            b.append([])
        for j in range(max(len(path) for path in pathss)):
            for i in range(length):
                if j<(len(pathss[i])):
                    a[i].append(pathss[i][j][0])
                    b[i].append(pathss[i][j][1])
                # print(f'第{i}个AGV的第{j}段路径')
                # print(a[i],b[i])

                    plt.scatter(a[i], b[i], s=20, color='black')
                else:
                    pass
                plt.pause(0.1)
                plt.draw()


        plt.ioff()
        plt.title(title)
        plt.show()

def a_star_path(G, start, end):
    # A*路径规划算法实现
    # 你可以使用networkx库内置的函数，例如nx.shortest_path()
    path = nx.shortest_path(G, source=start, target=end)
    return path  # 返回路径

def conflict_detection(AGVs, G):
    for i in range(len(AGVs)):
        for j in range(i + 1, len(AGVs)):
            if AGVs[i].path and AGVs[j].path:
                for pos in AGVs[i].path:
                    if pos in AGVs[j].path:
                        return True
    return False

def conflict_resolution(AGVs, G):
    for agv in AGVs:
        if agv.path:
            agv.path.pop(0)

def multi_run(G,AGVs,start,end):

    paths = []
    for agv in AGVs:
        agv.path = a_star_path(G, agv.start, agv.end)
        paths.append(agv.path)

    draw_graph(G,start,end, paths, "Initial Paths")

    while conflict_detection(AGVs, G):
        conflict_resolution(AGVs, G)

    paths = [agv.path for agv in AGVs]
    draw_graph(G, paths, "Final Paths")


def agv_init(n,start,end):
    AGVs=[]
    for i in range(n):
        print(i)
        agv = AGV(i, start[i], end[i])
        AGVs.append(agv)
    return AGVs


if __name__ == "__main__":
    from Map_maker import *
    #############################################
    # AGV参数配置
    start=[(21.838905, 15.613314),(20.678572, 20.333778)]
    end=[(24.082426, 22.923149),(39.3979, 27.973629)]
    n=2
    #############################################
    AGVs=agv_init(n,start,end)
    multi_run(G,AGVs,start,end)
